capture_motion.h

Go to the documentation of this file.

// WOLF - Copyright (C) 2020-2026
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
 
// Wolf includes
#include "core/capture/capture_base.h"
#include "core/processor/motion_buffer.h"
 
// STL includes
#include <list>
#include <algorithm>
#include <iterator>
#include <utility>
 
namespace wolf
{
WOLF_PTR_TYPEDEFS(CaptureMotion);
 
class CaptureMotion : public CaptureBase
{
  public:
    CaptureMotion(const std::string&     _type,
                  const TimeStamp&       _ts,
                  SensorBasePtr          _sensor_ptr,
                  const Eigen::VectorXd& _data,
                  CaptureBasePtr         _capture_origin_ptr,
                  const TypeComposite&   _state_types   = {},
                  const VectorComposite& _state_vectors = {},
                  const PriorComposite&  _state_priors  = {});
 
    CaptureMotion(const std::string&     _type,
                  const TimeStamp&       _ts,
                  SensorBasePtr          _sensor_ptr,
                  const Eigen::VectorXd& _data,
                  const Eigen::MatrixXd& _data_cov,
                  CaptureBasePtr         _capture_origin_ptr,
                  const TypeComposite&   _state_types   = {},
                  const VectorComposite& _state_vectors = {},
                  const PriorComposite&  _state_priors  = {});
 
    ~CaptureMotion() override;
 
    // Type
    bool isMotion() const override
    {
        return true;
    }
 
    // Data
    const Eigen::VectorXd& getData() const;
    const Eigen::MatrixXd& getDataCovariance() const;
    void                   setData(const Eigen::VectorXd& _data);
    void                   setDataCovariance(const Eigen::MatrixXd& _data_cov);
 
    // Buffer
    MotionBuffer&       getBuffer();
    const MotionBuffer& getBuffer() const;
 
    // Buffer's initial conditions for pre-integration
    Eigen::VectorXd getCalibrationPreint() const;
    void            setCalibrationPreint(const Eigen::VectorXd& _calib_preint);
    Eigen::MatrixXd getJacobianCalib() const;
    Eigen::MatrixXd getJacobianCalib(const TimeStamp& _ts) const;
 
    // Get delta preintegrated, and corrected for changes on calibration params
    Eigen::VectorXd getDeltaPreint() const;
    Eigen::VectorXd getDeltaPreint(const TimeStamp& _ts) const;
    Eigen::MatrixXd getDeltaPreintCov() const;
    Eigen::MatrixXd getDeltaPreintCov(const TimeStamp& _ts) const;
 
    // Origin frame and capture
    CaptureBasePtr      getOriginCapture();
    CaptureBaseConstPtr getOriginCapture() const;
    void                setOriginCapture(CaptureBasePtr _capture_origin_ptr);
 
    bool containsTimeStamp(const TimeStamp& _ts, double _time_tolerance) const;
 
    void printHeader(int           depth,        //
                     bool          factored_by,  //
                     bool          metric,       //
                     bool          state_blocks,
                     std::ostream& stream,
                     std::string   _tabs = "") const override;
 
    // member data:
  private:
    Eigen::VectorXd data_;                
    Eigen::MatrixXd data_cov_;            
    Eigen::VectorXd calib_preint_;        
    MotionBuffer    buffer_;              
    CaptureBaseWPtr capture_origin_ptr_;  
};
 
inline const Eigen::VectorXd& CaptureMotion::getData() const
{
    return data_;
}
 
inline const Eigen::MatrixXd& CaptureMotion::getDataCovariance() const
{
    return data_cov_;
}
 
inline void CaptureMotion::setData(const Eigen::VectorXd& _data)
{
    assert(_data.size() == data_.size() && "Wrong size of data vector!");
    data_ = _data;
}
 
inline void CaptureMotion::setDataCovariance(const Eigen::MatrixXd& _data_cov)
{
    assert(_data_cov.rows() == data_cov_.rows() && "Wrong number of rows of data vector!");
    assert(_data_cov.cols() == data_cov_.cols() && "Wrong number of cols of data vector!");
    data_cov_ = _data_cov;
}
 
inline const MotionBuffer& CaptureMotion::getBuffer() const
{
    return buffer_;
}
 
inline MotionBuffer& CaptureMotion::getBuffer()
{
    return buffer_;
}
 
inline Eigen::MatrixXd CaptureMotion::getJacobianCalib() const
{
    return getBuffer().back().jacobian_calib_;
}
 
inline Eigen::MatrixXd CaptureMotion::getJacobianCalib(const TimeStamp& _ts) const
{
    return getBuffer().getMotion(_ts).jacobian_calib_;
}
 
inline wolf::CaptureBasePtr CaptureMotion::getOriginCapture()
{
    return capture_origin_ptr_.lock();
}
 
inline wolf::CaptureBaseConstPtr CaptureMotion::getOriginCapture() const
{
    return capture_origin_ptr_.lock();
}
 
inline void CaptureMotion::setOriginCapture(CaptureBasePtr _capture_origin_ptr)
{
    capture_origin_ptr_ = _capture_origin_ptr;
}
 
inline Eigen::VectorXd CaptureMotion::getCalibrationPreint() const
{
    return calib_preint_;
}
 
inline void CaptureMotion::setCalibrationPreint(const Eigen::VectorXd& _calib_preint)
{
    calib_preint_ = _calib_preint;
}
 
inline Eigen::VectorXd CaptureMotion::getDeltaPreint() const
{
    return getBuffer().back().delta_integr_;
}
 
inline Eigen::VectorXd CaptureMotion::getDeltaPreint(const TimeStamp& _ts) const
{
    return getBuffer().getMotion(_ts).delta_integr_;
}
 
inline Eigen::MatrixXd CaptureMotion::getDeltaPreintCov() const
{
    return getBuffer().back().delta_integr_cov_;
}
 
inline Eigen::MatrixXd CaptureMotion::getDeltaPreintCov(const TimeStamp& _ts) const
{
    return getBuffer().getMotion(_ts).delta_integr_cov_;
}
 
}  // namespace wolf